#include "industrial_robot_control/modbus/modbus_client.h"
#include <modbus/modbus.h>
#include <thread>
#include "industrial_robot_control/utils/log.h"

namespace IndustrialRobotControl {
namespace Modbus {

ModbusClient::ModbusClient() : ctx_(nullptr), connected_(false) {}

ModbusClient::~ModbusClient() {
    disconnect();
}

bool ModbusClient::connect(const std::string& ip_address, int port) {
    LOG_INFO("Connecting to Modbus server at {}:{}", ip_address, port);
    
    // 断开现有连接
    if (connected_) {
        disconnect();
    }
    
    // 创建Modbus TCP上下文
    ctx_ = modbus_new_tcp(ip_address.c_str(), port);
    if (!ctx_) {
        LOG_ERROR("Failed to create Modbus context: {}", modbus_strerror(errno));
        return false;
    }
    
    // 设置超时
    struct timeval timeout;
    timeout.tv_sec = 1;
    timeout.tv_usec = 0;
    modbus_set_response_timeout(ctx_, &timeout);
    
    // 连接到服务器
    if (modbus_connect(ctx_) == -1) {
        LOG_ERROR("Failed to connect to Modbus server: {}", modbus_strerror(errno));
        modbus_free(ctx_);
        ctx_ = nullptr;
        return false;
    }
    
    connected_ = true;
    LOG_INFO("Successfully connected to Modbus server");
    return true;
}

void ModbusClient::disconnect() {
    if (connected_ && ctx_) {
        LOG_INFO("Disconnecting from Modbus server");
        modbus_close(ctx_);
        modbus_free(ctx_);
        ctx_ = nullptr;
        connected_ = false;
    }
}

bool ModbusClient::read_coils(int addr, int num, uint8_t* dest) {
    if (!connected_) {
        LOG_ERROR("Not connected to Modbus server");
        return false;
    }
    
    int rc = modbus_read_coils(ctx_, addr, num, dest);
    if (rc != num) {
        LOG_ERROR("Failed to read coils: {}", modbus_strerror(errno));
        return false;
    }
    return true;
}

bool ModbusClient::read_discrete_inputs(int addr, int num, uint8_t* dest) {
    if (!connected_) {
        LOG_ERROR("Not connected to Modbus server");
        return false;
    }
    
    int rc = modbus_read_discrete_inputs(ctx_, addr, num, dest);
    if (rc != num) {
        LOG_ERROR("Failed to read discrete inputs: {}", modbus_strerror(errno));
        return false;
    }
    return true;
}

bool ModbusClient::read_holding_registers(int addr, int num, uint16_t* dest) {
    if (!connected_) {
        LOG_ERROR("Not connected to Modbus server");
        return false;
    }
    
    int rc = modbus_read_holding_registers(ctx_, addr, num, dest);
    if (rc != num) {
        LOG_ERROR("Failed to read holding registers: {}", modbus_strerror(errno));
        return false;
    }
    return true;
}

bool ModbusClient::read_input_registers(int addr, int num, uint16_t* dest) {
    if (!connected_) {
        LOG_ERROR("Not connected to Modbus server");
        return false;
    }
    
    int rc = modbus_read_input_registers(ctx_, addr, num, dest);
    if (rc != num) {
        LOG_ERROR("Failed to read input registers: {}", modbus_strerror(errno));
        return false;
    }
    return true;
}

bool ModbusClient::write_single_coil(int addr, int status) {
    if (!connected_) {
        LOG_ERROR("Not connected to Modbus server");
        return false;
    }
    
    if (modbus_write_bit(ctx_, addr, status) == -1) {
        LOG_ERROR("Failed to write coil: {}", modbus_strerror(errno));
        return false;
    }
    return true;
}

bool ModbusClient::write_single_register(int addr, uint16_t value) {
    if (!connected_) {
        LOG_ERROR("Not connected to Modbus server");
        return false;
    }
    
    if (modbus_write_register(ctx_, addr, value) == -1) {
        LOG_ERROR("Failed to write register: {}", modbus_strerror(errno));
        return false;
    }
    return true;
}

} // namespace Modbus
} // namespace IndustrialRobotControl
